// Flattened model manually produced to simulate the system
//

macro timeOutCoarse = 0.05;

// K fine
macro KF = [0.1020408, 0.4081633, 26.63102, 4.2040816];

// K gross
macro KG = [417.95918, 208.97954, 613.55954, 136.4898];

macro massCart = 2.0;
macro lengthRod = 0.5;
macro massRod = 0.1;

module FlattenedPlantController(hybrid real position, hybrid real velocity, hybrid real angle, hybrid real angularVelocity, hybrid real force, event ?modeChange) {

	bool modeFine;
	hybrid real t;	

	t = 0.0;
	modeFine = true;
	
	while(true) {
		next(t) = 0.0;

		if (modeChange) next(modeFine) = not modeFine; else next(modeFine) = modeFine;  

		if ((modeChange and not modeFine) or (not modeChange and modeFine))
			next(force) = position * KF[0] + velocity * KF[1] + angle * KF[2] + angularVelocity * KF[3];
		else
			next(force) = position * KG[0] + velocity * KG[1] + angle * KG[2] + angularVelocity * KG[3];

		flow {} until (true);

		flow {
		    drv(t) <- 1.0;	
		    drv(force) <- 0.0;
		    drv(position) <- cont(velocity);
		    drv(angle) <- cont(angularVelocity);
		    drv(velocity) <- ((cont(force) - massRod * 9.81 * cont(angle))/ massCart);
		    drv(angularVelocity) <- (((massCart + massRod) * 9.81 * cont(angle) -cont(force))/ (massCart * lengthRod));
		} until (cont(t) >= timeOutCoarse);
	}
}